// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ReadInputRegistersPacket.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.Modbus
{
    using System;
    using System.Net.Sockets;
    using System.Runtime.Serialization;

    /// <summary>
    /// Functionality for ReadInputRegisters Modbus operation
    /// </summary>
    public static class ReadInputRegistersPacket
    {
        /// <summary>
        /// Length parameter for the read input registers packet
        /// </summary>
        public const ushort ReadInputRegistersLength = 6;

        /// <summary>
        /// Modbus TCP function number for Read Input Register
        /// </summary>
        public const byte ReadInputRegisterFunctionNumber = 4;

        /// <summary>
        /// Populate register values from a response to ReadInputRegisters
        /// </summary>
        /// <param name="transactionID">Expected transaction id.</param>
        /// <param name="buffer">Buffer containing packet.</param>
        /// <param name="registers">Out parameter to return registers.</param>
        public static void ParseReadInputRegistersResponse(byte[] transactionID, byte[] buffer, out ushort[] registers)
        {
            int currentByte = 0;

            // check that we recieved the expected transaction ID
            if (buffer[currentByte++] != transactionID[1])
            {
                throw new ApplicationException(
                    "Unexpected transaction ID byte #1, expected: " + transactionID[1] + ", received: " + buffer[currentByte - 1]);
            }

            if (buffer[currentByte++] != transactionID[0])
            {
                throw new ApplicationException(
                    "Unexpected transaction ID byte #2, expected: " + transactionID[0] + ", received: " + buffer[currentByte - 1]);
            }

            // check that protocol is Modbus TCP
            if (buffer[currentByte++] != ModbusTCPPacket.ModbusTCPProtocolIDSecondByte)
            {
                throw new ApplicationException(
                    "Unexpected protocol ID byte #1, expected: " + ModbusTCPPacket.ModbusTCPProtocolIDSecondByte + ", received: " + buffer[currentByte - 1]);
            }

            if (buffer[currentByte++] != ModbusTCPPacket.ModbusTCPProtocolIDFirstByte)
            {
                throw new ApplicationException(
                    "Unexpected protocol ID byte #2, expected: " + ModbusTCPPacket.ModbusTCPProtocolIDFirstByte + ", received: " + buffer[currentByte - 1]);
            }

            // skip length packet length (data length is later)
            currentByte += ModbusTCPPacket.PacketLengthByteCount;

            // skip unit ID
            currentByte++;

            // confirm function number
            if (buffer[currentByte++] != ReadInputRegisterFunctionNumber)
            {
                throw new ApplicationException(
                    "Expected function number: " + ReadInputRegisterFunctionNumber + ", received: " + buffer[currentByte - 1]);
            }

            // Length is returned here in bytes
            byte numRegisters = (byte)(buffer[currentByte++] / ModbusTCPPacket.ByteCountPerWord);

            registers = new ushort[numRegisters];

            for (int i = 0; i < numRegisters; ++i)
            {
                registers[i] = BitConverter.ToUInt16(buffer, currentByte);
                currentByte += ModbusTCPPacket.ByteCountPerWord;
            }
        }

        /// <summary>
        /// Builds a ReadInputRegisters request
        /// </summary>
        /// <param name="unitID">ID of the slave device.</param>
        /// <param name="addressOfFirstRegister">Address of the first register to read.</param>
        /// <param name="wordsToRead">Number of 16-bit words to read.</param>
        /// <param name="transactionID">Transaction ID.</param>
        /// <param name="buffer">Buffer (pre-allocated) to place packet into.</param>
        /// <returns> Size of packet in bytes. </returns>
        public static int BuildReadInputRegistersRequest(
            byte unitID,
            ushort addressOfFirstRegister,
            ushort wordsToRead,
            byte[] transactionID,
            byte[] buffer)
        {
            int currentByte = 0;

            if (!BitConverter.IsLittleEndian)
            {
                throw new ApplicationException("Expecting little endian machine");
            }

            // Robotiq S Model is little-endian, assumption we are on a big-endian
            // system so need to swaps byte order.
            buffer[currentByte++] = transactionID[1];
            buffer[currentByte++] = transactionID[0];

            // Protocol ID
            ModbusTCPPacket.CopyBytesInReverseOrder(
                ModbusTCPPacket.ModbusTCPProtocolID,
                buffer,
                currentByte);
            currentByte += ModbusTCPPacket.ProtocolIdentifierByteCount;

            // Length - will always be 6 for function 4.
            ModbusTCPPacket.CopyBytesInReverseOrder(
                ReadInputRegistersLength,
                buffer,
                currentByte);
            currentByte += ModbusTCPPacket.PacketLengthByteCount;

            // UnitID
            buffer[currentByte++] = unitID;

            // Function ID
            buffer[currentByte++] = ReadInputRegisterFunctionNumber;

            // Address of the first register
            ModbusTCPPacket.CopyBytesInReverseOrder(
                addressOfFirstRegister,
                buffer,
                currentByte);
            currentByte += ModbusTCPPacket.RegisterAddressByteCount;

            // Word count
            ModbusTCPPacket.CopyBytesInReverseOrder(
                wordsToRead,
                buffer,
                currentByte);
            currentByte += ModbusTCPPacket.ByteCountPerWord;

            return currentByte;
        }
    }
}
